/*
 * PID_Control.h
 *
 * Created on: $(date:yyyy-MM-dd)
 * Author: $(user)
 */

#ifndef PID_Contro_H
#define PID_Contro_H

// Add your function declarations here
#include "common.h"
#include "Device.hpp"

/* 控制器1参数 */
#define PID1_KP  2.0f      // 比例增益系数（决定控制器对误差的响应速度）
#define PID1_KI  0.5f      // 积分增益系数（消除稳态误差）
#define PID1_KD  0.25f     // 微分增益系数（抑制系统超调）

#define PID1_TAU 0.02f     // 微分时间常数（用于滤波，避免高频噪声干扰）

#define PID1_LIM_MIN -8.0f  // 机器俯仰角最小值限制
#define PID1_LIM_MAX  8.0f  // 机器俯仰角最大值限制

#define PID1_LIM_MIN_INT -4.0f  // 积分项最小值限制（防止积分饱和）
#define PID1_LIM_MAX_INT  4.0f  // 积分项最大值限制（防止积分饱和）

/* 控制器2参数 */
#define PID2_KP  2.0f      // 比例增益系数（决定控制器对误差的响应速度）
#define PID2_KI  0.5f      // 积分增益系数（消除稳态误差）
#define PID2_KD  0.25f     // 微分增益系数（抑制系统超调）

#define PID2_TAU 0.02f     // 微分时间常数（用于滤波，避免高频噪声干扰）

#define PID2_LIM_MIN -10.0f  // 俯仰角最小值限制
#define PID2_LIM_MAX  10.0f  // 俯仰角最大值限制

#define PID2_LIM_MIN_INT -5.0f  // 积分项最小值限制（防止积分饱和）
#define PID2_LIM_MAX_INT  5.0f  // 积分项最大值限制（防止积分饱和）

/* 速度控制器参数 */
#define PID_velocity_KP  2.0f      // 比例增益系数（决定控制器对误差的响应速度）
#define PID_velocity_KI  0.5f      // 积分增益系数（消除稳态误差）
#define PID_velocity_KD  0.25f     // 微分增益系数（抑制系统超调）

#define PID_velocity_TAU 0.02f     // 微分时间常数（用于滤波，避免高频噪声干扰）

#define PID_velocity_LIM_MIN -100.0f  // 螺旋桨转速最小值限制
#define PID_velocity_LIM_MAX  100.0f  // 螺旋桨转速最大值限制

#define PID_velocity_LIM_MIN_INT -50.0f  // 积分项最小值限制（防止积分饱和）
#define PID_velocity_LIM_MAX_INT  50.0f  // 积分项最大值限制（防止积分饱和）

/* 航向角控制器参数 */
#define PID_heading_KP  2.0f      // 比例增益系数（决定控制器对误差的响应速度）
#define PID_heading_KI  0.5f      // 积分增益系数（消除稳态误差）
#define PID_heading_KD  0.25f     // 微分增益系数（抑制系统超调）

#define PID_heading_TAU 0.02f     // 微分时间常数（用于滤波，避免高频噪声干扰）

#define PID_heading_LIM_MIN -7.0f  // 左右转角最小值限制
#define PID_heading_LIM_MAX  7.0f  // 左右转角最大值限制

#define PID_heading_LIM_MIN_INT -3.50f  // 积分项最小值限制（防止积分饱和）
#define PID_heading_LIM_MAX_INT  3.50f  // 积分项最大值限制（防止积分饱和）

#define SAMPLE_TIME_S 0.01    // 采样时间（单位：秒，控制算法更新频率）

/* 仿真运行的最大时间 */
#define SIMULATION_TIME_MAX 4  // 仿真总时长为4秒


/**
 * @brief 定点控制
 * @param  end_x
 * @param  end_y
 * @param  device_propeller
 * @return int 
 * @author Dongcj (1217365417@qq.com)
 * @date 2025-07-21
*/
int ToPoint(float end_x, float end_y, Device<Propeller> &device_propeller);

/**
 * @brief 定深控制
 * @param  setpoint
 * @param  device_propeller
 * @return int 
 * @author Dongcj (1217365417@qq.com)
 * @date 2025-07-21
*/
int ToDepth(float setpoint, Device<Propeller> &device_propeller);

/**
 * @brief 定航向控制
 * @param  setpoint 目标航向角
 * @return int 
 * @author Dongcj (1217365417@qq.com)
 * @date 2025-07-20
*/
int ToHeading(float setpoint, Device<Propeller>& device_propeller);

/**
 * @brief 定速度控制
 * @param  setpoint 目标速度
 * @return int 
 * @author Dongcj (1217365417@qq.com)
 * @date 2025-07-20
*/
int ToVelocity(float setpoint, Device<Propeller>& device_propeller);

#endif // PID_Contro_H